#include "ArcBall.hpp"

namespace zzz{
ArcBall::ArcBall()
{
  rot.Identical();   
  anchor_rot.Identical();
  comb.Identical();
  trans.Identical();
  inv_rotate=false;
  combine=false;
  mode2d=false;
}  

void ArcBall::SetWindowSize(double w, double h)
{
  win_w = w;
  win_h = h;
}

void ArcBall::SetOldPos(double x, double y)
{            
  old_x = x;
  old_y = y;
  Bind();
}

void ArcBall::Rotate(double x, double y)
{            
  double ox, oy, cx, cy;
  ox = old_x;
  oy = old_y;
  cx = x;
  cy = y;

  Normalize(ox, oy);
  Normalize(cx, cy);

  Quaternion<double> incr;
  if (mode2d)
  {
    double old_theta=atan2(oy,ox);
    double theta=atan2(cy,cx);
    incr.SetAxisAngle(Vector<3,double>(0,0,1),theta-old_theta);
    rot = incr*anchor_rot;
  }
  else
  {
    Vector<3,double> oldp(ox, oy, 0.0), currp(cx, cy, 0.0);

    ProjectOntoSurface(oldp);
    ProjectOntoSurface(currp);

    //this is not axis and angle, is quaternion values
    Quaternion<double> q1(oldp[0],oldp[1],oldp[2], 0.0);
    Quaternion<double> q2(currp[0],currp[1],currp[2], 0.0);

    if (inv_rotate)
    {
      incr = q1*q2;
      rot = anchor_rot*incr;
    }
    else
    {
      incr = q2*q1;
      if(combine)
        rot = incr*anchor_rot*comb;
      else
        rot = incr*anchor_rot;
    }
  }

  rot.Normalize();
  trans=Transformation<double>(Rotation<double>(rot));
}

void ArcBall::Reset()
{
  rot.Identical();   
  anchor_rot.Identical();
  trans.Identical();
}

Vector<3,double> ArcBall::GetAxis()
{
  return rot.Axis();
}   

double ArcBall::GetAngle()
{
  return rot.Angle();
}

const GLTransformation<double>& ArcBall::GetGLTransform() const
{
  return trans;
}

const double* ArcBall::GetGLRotateMatrix() const
{
  return trans.Data();
}

void ArcBall::SetInverse(bool val)
{
  inv_rotate = val;
} 

void ArcBall::CombineRotation(const Quaternion<double> &q)
{
  comb = q;
  combine = true;
}

void ArcBall::StopCombineRotation()
{
  combine = false;
}


const Quaternion<double> ArcBall::GetQuaternion() const
{
  return rot;
} 

void ArcBall::SetMode2D(bool val)
{
  mode2d=val;
}

bool ArcBall::GetMode2D()
{
  return mode2d;
}

void ArcBall::ApplyGL() const
{
  GetGLTransform().ApplyGL();
}
/// anchors the current rotation
void ArcBall::Bind()
{
  anchor_rot = rot;    
}

void ArcBall::Normalize(double &x, double &y)
{
  x = 2.0 * x / win_w  - 1.0;
  if (x < -1.0) x = -1.0;
  if (x >  1.0) x =  1.0;

  y = -(2.0 * y / win_h - 1.0);
  if (y < -1.0) y = -1.0;
  if (y >  1.0) y =  1.0;
}

void ArcBall::ProjectOntoSurface(Vector<3,double> &v)
{  
  double radius2 = 1.0;
  double len2 = v[0]*v[0] + v[1]*v[1];

  if (len2 <= radius2 / 2.0)
    v[2] = sqrt(radius2 - len2);
  else 
    v[2] = radius2 / (2 * sqrt(len2));
  
  len2 = sqrt(len2 + v[2]*v[2]);    
  for (int i = 0; i < 3; ++i) v[i] /= len2;
}
}
